#include "vision.h"

#include "base_drv/drv_conf.h"
#include HAL_INCLUDE

#include <string.h>
#include <math.h>

#include "base_drv/referee.h"
#include "base_drv/motor_ctrl.h"
#include "algorithm/ballistic.h"
#include "algorithm/imu_fusion.h"
#include "algorithm/kalman.h"
#include "algorithm/filter.h"

#include "algorithm/crc.h"
#include "algorithm/util.h"

#include "shoot.h"
#include "fire_control.h"

#if VISION_INF_USB == 0
#include "base_drv/drv_uart.h"

int32_t vision_frame_offset = 0;
int32_t vision_receive_size = 0;

uint8_t vision_rx_buffer[VISION_CTRL_FRAME_LEN*2];

extern UART_HandleTypeDef VISION_UART_HANDLE;
#else

#include "../Inc/usbd_cdc_if.h"

#endif

uint16_t vision_rx_lost = VISION_RX_LOST_MAX;
uint8_t vision_req_flag = 0;

vision_req_t vision_request;
vision_ctrl_t vision_ctrl_data;


uint8_t current_task_mode = MODE_AUTO_AIM;
uint8_t armour_state = NONARMOR;

float current_pitch, current_yaw;
float vision_pitch, vision_yaw;


#define INIT_SHOOT_SPEED 25.0f

float predict_time = 0.0f, init_predict_time = 0.0f;
float cur_v0 = INIT_SHOOT_SPEED;
extern eular_t _imu_eular;
float distance_xy;

target_spec_t target;
ballistic_sol_t solution;

#define MAX_PREDICT_T 0.8f
#define MAX_TARGET_LOST 60
#define CONST_DELAY 0.09f
float est_x,est_y,est_z;
float const_delay = CONST_DELAY;



kalman1_state ks = {
	.x = INIT_SHOOT_SPEED,
    .p = 0.0f,
    .A = 1,
    .H = 1,
    .q = 0.02f,//10e-6;  /* predict noise convariance */
    .r = 100.0f//10e-5;  /* measure error convariance */
};


/**
  * @brief      return the self color for vision
  * @return     0 for red, others for blue
  */
uint8_t is_red_or_blue(void){
	return (game_robot_status.robot_id < 100);
}

void set_task_mode(uint8_t tm){ current_task_mode = tm; }

//uint8_t get_vision_cmd(void){ return vision_control_data.command;}

void set_vision_req_ang(float pitch, float yaw){
	current_pitch = pitch;
	current_yaw = yaw;
}



#if VISION_INF_USB == 0
HAL_StatusTypeDef vision_uart_dma_recv_init(void){
	vision_rx_lost = VISION_RX_LOST_MAX;
		
	__HAL_UART_ENABLE_IT(&(VISION_UART_HANDLE),UART_IT_IDLE);
	return uart_recv_dma_init(&(VISION_UART_HANDLE),vision_rx_buffer,VISION_CTRL_FRAME_LEN*2);
}

void vision_uart_idle_handler(UART_HandleTypeDef* huart){
	if(huart->Instance == VISION_UART && __HAL_UART_GET_FLAG(huart,UART_FLAG_IDLE)){
		__HAL_UART_CLEAR_IDLEFLAG(huart);
		DMA_Stream_TypeDef* uhdma = huart->hdmarx->Instance;
		vision_receive_size = (int32_t)(2*VISION_CTRL_FRAME_LEN - uhdma->NDTR) - vision_frame_offset;
		if(vision_receive_size == VISION_CTRL_FRAME_LEN || vision_receive_size == -VISION_CTRL_FRAME_LEN){
			if(parse_vision_data(&vision_rx_buffer[vision_frame_offset],VISION_CTRL_FRAME_LEN) == VISION_DATA_NOERR){
				vision_rx_lost = 0;
			}
			vision_frame_offset = (int32_t)(vision_frame_offset == 0)*VISION_CTRL_FRAME_LEN;
		}else if(vision_frame_offset != 0){
			//some bytes lost, reset DMA buffer
			CLEAR_BIT(huart->Instance->CR3, USART_CR3_DMAR);
			__HAL_DMA_DISABLE(huart->hdmarx);
			vision_frame_offset = 0;
			uhdma->NDTR = (uint32_t)(VISION_CTRL_FRAME_LEN*2);
			__HAL_DMA_CLEAR_FLAG(huart->hdmarx, __HAL_DMA_GET_TC_FLAG_INDEX(huart->hdmarx));
			__HAL_DMA_CLEAR_FLAG(huart->hdmarx, __HAL_DMA_GET_HT_FLAG_INDEX(huart->hdmarx));
			__HAL_DMA_CLEAR_FLAG(huart->hdmarx, __HAL_DMA_GET_TE_FLAG_INDEX(huart->hdmarx));
			__HAL_DMA_ENABLE(huart->hdmarx);
			SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
		}
	}
}
#else

uint8_t parse_vision_data(uint8_t* buf, uint16_t len){
	if(buf[0] == VISION_UART_CTRL_SOF && len == VISION_CTRL_FRAME_LEN &&
		Verify_CRC16_Check_Sum(&buf[0], VISION_CTRL_FRAME_LEN)){	
			memcpy((uint8_t*)&vision_ctrl_data, buf, VISION_CTRL_FRAME_LEN);			
		if(vision_ctrl_data.x != vision_ctrl_data.x ||vision_ctrl_data.y != vision_ctrl_data.y || vision_ctrl_data.z != vision_ctrl_data.z)
		{
			return VISION_DATA_ERR;
		}	
		if(vision_ctrl_data.target_found){
			if(cur_v0 != 0.0f){
				init_predict_time = target.x0/(cur_v0*cosf(_imu_eular.pitch*(2.0f*PI/ANGLE_RANGE))) + const_delay;
			}else init_predict_time = const_delay;
			LIMIT_MIN_MAX(init_predict_time,0.0f,MAX_PREDICT_T);
			predict_time = init_predict_time;
		}else{
			predict_time = 0.0f;
		}
		
		vision_rx_lost = 0;
		return VISION_DATA_NOERR;
	}else return VISION_DATA_ERR;
}
// uint8_t parse_vision_data1(uint8_t* buf, uint16_t len){
//	if(buf[0] == VISION_UART_CTRL_SOF && len == VISION_CTRL_FRAME_LEN1 &&
//		Verify_CRC16_Check_Sum(&buf[0], VISION_CTRL_FRAME_LEN1)){
//		memcpy((uint8_t*)&vision_control_data1, buf, VISION_CTRL_FRAME_LEN1);
////		
////		if(shoot_data.bullet_speed > 0.1f) cur_v0 = kalman1_filter(&ks,shoot_data.bullet_speed);
////			
////		if(cur_v0 != 0.0f){
////			init_predict_time = target.x0/(cur_v0*cosf(_imu_eular.pitch*(2.0f*PI/ANGLE_RANGE))) + CONST_SHOOT_DELAY;
////		}else init_predict_time = CONST_SHOOT_DELAY;

////		LIMIT_MIN_MAX(init_predict_time,0.0f,MAX_PREDICT_T);
////		
////		predict_time = init_predict_time;
//		vision_rx_lost = 0;
//		return VISION_DATA_NOERR;
//	}else return VISION_DATA_ERR;
//}
#endif


uint8_t vision_ctrl(float* pitch_ang, float* yaw_ang, float dt)
{
	if(vision_ctrl_data.target_found)
	{
		select_amor(&vision_ctrl_data,&est_x,&est_y,&est_z);

		if(pitch_ang != NULL && yaw_ang != NULL){
			distance_xy = sqrtf(est_x*est_x + est_y*est_y);
			
			*yaw_ang = atan2f(est_y,est_x)*ANGLE_RANGE/(2.0f*PI);
			
			target.x0 = distance_xy;target.z0 = est_z;
		//target.x0 = 3.0; target.z0 = -0.425+0.126;//m
			 			 
			projectile_solve(cur_v0, &target, &solution);
			
			if(solution.solution_num > 0){
				if(solution.ang_solution1 < solution.ang_solution2){
					*pitch_ang  = solution.ang_solution1*ANGLE_RANGE/(2.0f*PI);
				}else{
					*pitch_ang  = solution.ang_solution2*ANGLE_RANGE/(2.0f*PI);
				}
			}else{
				*pitch_ang = atan2f(est_z,distance_xy)*ANGLE_RANGE/(2.0f*PI);
			}					
		}
		return VISION_OK;
	} else {
		return VISION_NOTARGET;
	}
}

/**
  * @brief  send upper data
  * @return none
  */
void send_vision_request(float current_pitch, float current_yaw){
	//vision_request.f_sof = VISION_UART_REQ_SOF;
	//float shoot_speed = 15.0f;

//	vision_request.self_color = is_red_or_blue();
//	vision_request.task_mode = current_task_mode;
	
	
	vision_request.header = VISION_UART_REQ_SOF;
	vision_request.pitch = current_pitch;
	vision_request.yaw = current_yaw;
	vision_request.aim_x = est_x;
	vision_request.aim_y = est_y;
	vision_request.aim_z = est_z;
	vision_request.local_color = is_red_or_blue();
	vision_request.mode = 0;
	
	
//	if(vision_ctrl_data.task_mode == MODE_SMALL_BUFF ||
//		vision_ctrl_data.task_mode == MODE_LARGE_BUFF){
//		shoot_speed = 25.0f;
//	}else{
//		shoot_speed = game_robot_status.shooter_id1_17mm_speed_limit * SHOOT_SPEED_DEC;
//		if(shoot_speed < DEFAULT_SHOOT_SPEED * SHOOT_SPEED_DEC) 
//			shoot_speed = DEFAULT_SHOOT_SPEED * SHOOT_SPEED_DEC;
//	}
//	vision_request.shoot_speed[0] = (uint8_t)shoot_speed;
//	vision_request.shoot_speed[1] = ((uint8_t)(shoot_speed*10.0f)) % 10;
//	
//	vision_request._adjust = 0;
	vision_request.checksum = Get_CRC16_Check_Sum((uint8_t*)&vision_request
													,sizeof(vision_req_t)-2 , CRC16_INIT);
	#if VISION_INF_USB == 0
		uart_send(&(VISION_UART_HANDLE),(uint8_t*)&vision_request, sizeof(vision_req_t));
	#else
		CDC_Transmit_FS((uint8_t*)&vision_request, sizeof(vision_req_t));
	#endif
}

/**
  * @brief  check if vision upper is offline
  * @return 0 for  vision upper online, others for offline
  */
uint32_t is_vision_offline(void){ return vision_rx_lost >= VISION_RX_LOST_MAX; }


/**
  * @brief  increment of vision_rx_lost counter, should be call after process of all rc data
  * @return none
  */
void inc_vision_rx_lost(void){
	if(vision_rx_lost < VISION_RX_LOST_MAX) vision_rx_lost++;
}

/**
  * @brief  check vision request flag
  * @return 1 for vision upper request, others for not receive
  * @note calling this function will set vision_rx_flag to zero
  */
uint8_t is_vision_req(void){
	uint8_t rx = vision_req_flag;
	vision_req_flag = 0;
	return rx;
}


/**
  * @brief  get the current armour state
  * @return one of the value in e_ctrl_cmd
  */
uint8_t get_armour_state(void){ return armour_state; }

extern float sped[3];
void update_cur_v0(void)
{

	if (shoot_data.bullet_speed > 9.0f)
	{
		cur_v0 = mean_filter_2(sped[1]);
	}
}
